#pragma once
#include<iostream>
#include<opencv2/opencv.hpp>
#include<Armor.hpp>
#include<map>
#include<set>
using namespace std;
using namespace cv;

// 我的卡尔曼滤波是用10帧数据计算稳定的单帧时间间隔，然后预测下一帧的位置，而不是直接预测10帧后的位置。


class KalmanCalculate {
private:
    
    int stateSize;       // 状态向量维度
    int measSize;        // 观测向量维度

public:
    cv::KalmanFilter kf;    // Opnecv卡尔慢滤波指针
    // 卡尔曼初始化标记
    bool isFirstFrame;    
    float dt;            // 十帧经过平滑之后的数据
    
    KalmanCalculate(const Mat &rvec,const Mat &tvec){
        this->stateSize = 12;   // [tx, ty, tz, vx, vy, vz,rx, ry,rz,vrx,vry,vrz]
        this->measSize = 6;    // [tx, ty, tz]
        this->dt = tools::TimeStep;

        //初始化卡尔曼滤波器 
        this->kf = KalmanFilter(stateSize, measSize, 0, CV_32F); 

        // 初始化状态 [tx, ty, tz, vx, vy, vz, rx, ry, rz, vrx, vry, vrz]
        kf.statePost = (Mat_<float>(12,1) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2),  // tx, ty, tz
            0, 0, 0,  // vx, vy, vz
            rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2),  // rx, ry, rz
            0, 0, 0   // vrx, vry, vrz
        );



        // 状态转移矩阵（匀加速模型）
        setIdentity(this->kf.transitionMatrix);
        // 平移向量动态模型
        this->kf.transitionMatrix.at<float>(0, 3) = dt;  // tx += vx*dt
        this->kf.transitionMatrix.at<float>(1, 4) = dt;  // ty += vy*dt  
        this->kf.transitionMatrix.at<float>(2, 5) = dt;  // tz += vz*dt
        // 旋转向量动态模型
        this->kf.transitionMatrix.at<float>(6, 9) = dt;  // rx += vrx*dt
        this->kf.transitionMatrix.at<float>(7, 10) = dt; // ry += vry*dt
        this->kf.transitionMatrix.at<float>(8, 11) = dt; // rz += vrz*dt



        //  控制矩阵B 初始化为单位矩阵
        setIdentity(this->kf.controlMatrix);
        // 观测矩阵（观测平移和旋转向量）
        this->kf.measurementMatrix = (cv::Mat_<float>(6,12) <<
            1,0,0,0,0,0, 0,0,0,0,0,0,  // tx
            0,1,0,0,0,0, 0,0,0,0,0,0,  // ty
            0,0,1,0,0,0, 0,0,0,0,0,0,  // tz
            0,0,0,0,0,0, 1,0,0,0,0,0,  // rx
            0,0,0,0,0,0, 0,1,0,0,0,0,  // ry
            0,0,0,0,0,0, 0,0,1,0,0,0   // rz
        );


        // 5. 配置噪声参数（可根据场景调整）
        // 过程噪声 Q （模型不确定性）加速度突变带来的误差
        cv::setIdentity(this->kf.processNoiseCov, cv::Scalar::all(50));  // 10  100
        // initialProcessNoiseCov = kf->processNoiseCov.clone(); // 保存初始Q

        // 观测噪声 R（检测误差）检测算法的定位误差
        cv::setIdentity(this->kf.measurementNoiseCov, cv::Scalar::all(5));  // 10  5
        // initialMeasurementNoiseCov = kf->measurementNoiseCov.clone(); // 保存初始R

        // 初始后验误差协方差 P_pred
        cv::setIdentity(this->kf.errorCovPost, cv::Scalar::all(100));  // 初始状态不确定性 100 100
    };

    /* 拷贝构造*/
    KalmanCalculate(const KalmanCalculate &other){
        // 简单复制
        this->stateSize = other.stateSize;
        this->measSize = other.measSize;
        this->isFirstFrame = other.isFirstFrame;
        this->dt = other.dt;

        // 创建新的 KalmanFilter 对象
        this->kf = KalmanFilter(stateSize, measSize, 0, CV_32F);

        // 克隆
        this->kf.statePre = other.kf.statePre.clone();
        this->kf.statePost = other.kf.statePost.clone();
        this->kf.transitionMatrix = other.kf.transitionMatrix.clone();
        this->kf.controlMatrix = other.kf.controlMatrix.clone();
        this->kf.measurementMatrix = other.kf.measurementMatrix.clone();
        this->kf.processNoiseCov = other.kf.processNoiseCov.clone();
        this->kf.measurementNoiseCov = other.kf.measurementNoiseCov.clone();
        this->kf.errorCovPre = other.kf.errorCovPre.clone();
        this->kf.gain = other.kf.gain.clone();
        this->kf.errorCovPost = other.kf.errorCovPost.clone();

    };
    

    // 后验状态向量
    // 初始化目标初始状态
    void init(const Mat &rvec,const Mat &tvec);

    // // 获取先验状态下的tvec
    // Mat predictTvec();

    // // 获取先验状态下的rvec
    // Mat predictRvec();

    // 获取后验状态下的Tvec
    Mat getTvec();

    // 获取后验状态下的Rvec
    Mat getRvec();

    // 用当前观测值修正先验状态，最终得到修正后的最优状态（后验状态）
    // 用观测值更新滤波状态
    void correct(const cv::Mat& rvec, const cv::Mat& tvec);

    //更新状态矩阵
    void updateKalmanTracker(float time_step);

    // // 初始化过程噪声矩阵
    // void setProcessNoise(float dt);

    // 获取tvec速度
    Mat getTVelocity();

    // 获取rvec速度 
    Mat getRVelocity();

};





